Note: This tutorial assumes basic familiarity with the OUR robot..
(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Plan a trajectory to control an OUR robot and display synchronously in the Rviz

Description: Walks through the steps of creating a moveIt package for OUR_robot and using MoveGroup class for planning trajectories to control the OUR robot. Meanwhile, the Rviz also shows the motion as same as the real robot's.

Keywords: OUR trajectory rviz

Tutorial Level: BEGINNER

Pre-requisites

  • 1. Already install the ROS and MoveIt!

    • Follow the instructions for installing MoveIt! first if you have not already done that.

  • 2. Having the URDF for OUR_robot

Run moveIt Setup Assistant

Start

To start the MoveIt! Setup Assistant: roslaunch moveit_setup_assistant setup_assistant.launch This will bringup the start screen with two choices: Create New MoveIt! Configuration Package or Edit Existing MoveIt! Configuration Package. Click on the Create New MoveIt! Configuration Package button to bring up the following screen:

Click on the browse button and navigate to the UDRF Choose that file and then click Load Files. The Setup Assistant will load the files (this might take a few seconds) and present you with this screen

Generate Self-Collision Matrix

The Default Self-Collision Matrix Generator searches for pairs of links on the robot that can safely be disabled from collision checking, decreasing motion planning processing time. These pairs of links are disabled when they are always in collision, never in collision, in collision in the robot’s default position or when the links are adjacent to each other on the kinematic chain. The sampling density specifies how many random robot positions to check for self collision. Higher densities require more computation time while lower densities have a higher possibility of disabling pairs that should not be disabled. The default value is 10,000 collision checks. Collision checking is done in parallel to decrease processing time. Click on the Self-Collisions pane selector on the left-hand side and click on the Regenerate Default Collision Matrixbutton. The Setup Assistant will work for a few second before presenting you the results of its computation in the main table.

Add Virtual Joints

Virtual joints are used primarily to attach the robot to the world. For the PR2 we will define only one virtual joint attaching the base_footprint of the PR2 to the odom_combined world frame. This virtual joint represents the motion of the base of the robot in a plane. Click on the Virtual Joints pane selector. Click on Add Virtual Joint Set the joint name as “virtual_joint” Set the child link as “base” and the parent frame name as “odom_combined”. Set the Joint Type as “planar”. Click Save and you should see this screen:

Add Planning Groups

Planning groups are used for semantically describing different parts of your robot, such as defining what an arm is, or an end effector. Click on the Planning Groups pane selector. Click on Add Group and you should see the following screen: We will add the whole arm as a planning group

Add Robot Poses

The Setup Assistant allows you to add certain fixed poses into the configuration. This helps if, for example, you want to define a certain position of the robot as a Home position. Click on the Robot Poses pane. Click Add Pose. Choose a name for the pose. The robot will be in its Default position where the joint values are set to the mid-range of the allowed joint value range. Move the individual joints around until you are happy and then Save the pose. Note how poses are associated with particular groups. You can save individual poses for each group. IMPORTANT TIP: Try to move all the joints around. If there is something wrong with the joint limits in your URDF, you should be able to see it immediately here.

Label End Effectors

The OUR_robot does not have any end effectors so we will skip this step. STEP 7: Add Passive Joints The OUR_robot does not have any passive joints so we will skip this step.

Generate Configuration Files

You are almost there. One last step - generating all the configuration files that you will need to start using MoveIt! Click on the Configuration Files pane. Choose a location and name for the ROS package that will be generated containing your new set of configuration files. Click on the Generate Package button. The Setup Assistant will now generate and write a set of launch and config files into the directory of your choosing. All the generated files will appear in the Generated Files/Folders tab and you can click on each of them for a description of what they contain.

Using Move Group Interface to plan a trajectory sending to an OUR robot

  • We use the MoveGroup class to set joint or pose goal,creating motion plans, decomposing plans and sending the trajectory to driver node controling real robot.

  • (1) The MoveGroup class can be easily setup using just the name of the group you would like to control and plan for.

   1 moveit::planning_interface::MoveGroup group("joint_group");
  • (2) Getting Basic Information

   1   ROS_INFO("Reference frame: %s", group.getPlanningFrame().c_str());//print the name of the reference frame for this robot
   2   ROS_INFO("Reference frame: %s", group.getEndEffectorLink().c_str());//print the name of the end-effector link for this group
   3 
  • (3)

    • First get the current set of joint values for the group let’s modify one of the joints, plan to the new joint space goal we call the planner to compute the plan

   1   std::vector<double> group_variable_values;
   2     group.getCurrentState()->copyJointGroupPositions(group.getCurrentState()->getRobotModel()->getJointModelGroup(group.getName()), group_variable_values);
   3    
   4   group_variable_values[0] = 0;
   5   group_variable_values[1] = 20*M_PI/180.0;
   6   group_variable_values[2] = -70*M_PI/180.0;
   7   group_variable_values[3] = 0;
   8   group_variable_values[4] = -90*M_PI/180.0;
   9   group_variable_values[5] = 0;
  10   group.setJointValueTarget(group_variable_values);
  11   moveit::planning_interface::MoveGroup::Plan my_plan;
  12   bool success = group.plan(my_plan);
  • (4) Discompose the trajectory and publish to the topic named "moveJ"

   1   std_msgs::Float32MultiArray joints;
   2   joints.data.resize(6);
   3   ros::NodeHandle nh;
   4   ros::Publisher command_pub = nh.advertise<std_msgs::Float32MultiArray> ("moveJ", 1000);
   5   ros::Rate loop_rate(20);//Hz
   6   int j =0;
   7   while(ros::ok())
   8   {
   9       if( j != my_plan.trajectory_.joint_trajectory.points.size()) //or return and again
  10       {
  11           for(j=0; j<my_plan.trajectory_.joint_trajectory.points.size(); j++)
  12           {
  13               for(int i=0; i<6; i++)
  14              {
  15                   joints.data[i]=my_plan.trajectory_.joint_trajectory.points[j].positions[i];
  16                   ROS_INFO("send ponits[%d]positions[%d] %f",j,i,joints.data[i]);
  17              }
  18               command_pub.publish(joints);
  19               loop_rate.sleep();
  20           }
  21       }
  22   }

Wiki: Robots/OUR/Plan a trajectory to control an OUR robot and display synchronously the motion in the Rviz (last edited 2015-11-30 10:10:43 by MengNanLi)